#include "CfastCalculator.h"

#include <iostream>
#include <sstream>

#include "Utility/Logging/SGLogger.h"
#include "Utility/Configuration/SGConfig.h"
#include "Utility/Algebra/MathTools.h"
#include "Utility/Algebra/Matrix.h"
#include "Utility/Algebra/Tools.h"

#include "DataStructure/Input/Material/IsotropicMaterial.h"
#include "DataStructure/Input/Node/NodeData.h"
#include "DataStructure/Input/Property/PFASTData.h"
#include "DataStructure/Input/Element/CFASTData.h"

#include "DBManager/DBServiceFactory.h"
#include "DBManager/IService/Input/Property/IPFASTService.h"
#include "DBManager/IService/Input/Element/IElementService.h"
#include "DBManager/IService/Input/Material/IIsotropicMaterialService.h"
#include "DBManager/IService/Input/Node/INodeService.h"

#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/CoordinateProcessor.h"
#include "SGFem/Pre/Assembly/ModelInfoQuery.h"
#include "SGFem/Integration/ShapeFunction/ParaCoordCalctor.h"
#include "SGFem/Integration/ShapeFunction/Plane4NodeShpFunc.h"

#include "SGFem/ElementCalculator/Common/ElementTools.hpp"
#include "SGFem/ShellSecCalculator/ShellSectionParser.h" // 截面厚度解析
#include "SGFem/Constraint/RElement/RBE3Calculator.h"
#include "SGFem/ConnectElement/CweldElement/CweldCalculator.h"
#include "SGFem/ConnectElement/CweldElement/ConnectElementHleper.h"

using SG::Algebra::Matrix;
using SG::Algebra::Matrixd;
using SG::Algebra::Point;
using SG::Algebra::PointArray;
using SG::Algebra::Vector3D;
using SG::DataStructure::Common::Status;
using SG::DataStructure::Common::Id;
using SG::DataStructure::Common::IdArray;
using SG::DataStructure::Common::Real;
using SG::DataStructure::Common::TOLERANCE;
using namespace SG::DataStructure::FEM;
using namespace SG::DBManager;

using SG::DataStructure::FEM::IntegPointArray;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::CoordinateProcessor;
using SG::FEMSolver::Element::ShellSectionParser;
using SG::FEMSolver::Constraint::RBE3Calculator;
using SG::FEMSolver::Element::CweldCalculator;
using SG::FEMSolver::Element::CfastCalculator;
using SG::FEMSolver::Element::ConnectElementHleper;

CfastCalculator::CfastCalculator ()
{}

/** @brief 获取辅助点的在壳单元坐标系下的局部坐标
 *
 * @param pLocal             投影点局部坐标
 * @param Tshell             壳转换矩阵
 * @param T                  粱转换矩阵
 * @param a                  水平长度
 * @param[out] auxLocalCoord    辅助点局部Matrix类型
 * @author   yangqiuyou
 * @date     2025.04.09
 */
void CfastCalculator::ComputeAuxNode (const SG::Algebra::Point&         pLocal,
                               const SG::Algebra::Matrix&               Tshell,
                               const SG::Algebra::Matrix&               T,
                               const SG::DataStructure::Common::Real&   a,
                               _OUT SG::Algebra::Matrix&                auxLocalCoord)
{
    // 辅助点在梁投影框坐标系下的局部坐标
    auxLocalCoord  = Matrix (4, 2, {
        -a, -a,
         a, -a,
         a,  a,
        -a,  a});

    //壳法线向量
    Vector3D planeNormal = {Tshell(2, 0), Tshell(2, 1), Tshell(2, 2)};
    
    // 粱单元y方向轴矢量 投影到壳平面上 全局y-（y*n）* n
    Vector3D yAixBeam = {T(1, 0), T(1, 1), T(1, 2)};
    Real len = dot(yAixBeam, planeNormal);
    Vector3D yNew = yAixBeam - (planeNormal * len);
    
    // yNew 在壳面上投影为一个点，令新轴和壳的坐标系相同
    if(yNew.IsZero())
    {
        yNew = {1.0, 0.0, 0.0};
    }    
    yNew.Normalize ();

    // 坐标系轴在 壳单元坐标系下分量
    auto yNewLoc = yNew.Rotate(Tshell.Transpose());

    // 获得粱投影坐标系和 壳坐标系的转换矩阵
    // 此时 壳坐标基矢量在自身坐标系下为 {1.0 ，0} 和 {0.0， 1.0}
    const auto cos = yNewLoc.m_x;
    const auto sin = yNewLoc.m_y;
    Matrix Trotation(2, 2, {cos, sin, -sin, cos});

    // 将辅助点从梁投影坐标系转动到 壳单元局部坐标系
    auxLocalCoord = auxLocalCoord * Trotation;

    // 辅助点在壳单元局部坐标下分量
    const auto x = pLocal.m_x;
    const auto y = pLocal.m_y;
    for (std::size_t i = 0; i < 4; ++i)
    {
        auxLocalCoord(i, 0) += x;
        auxLocalCoord(i, 1) += y;
    }
}

void CfastCalculator::Initialize (SG::DataStructure::FEM::ElementTypes type)
{
    // do nothing
}

/** @brief CFAST单元刚度矩阵计算
 *
 * @param L           单元长度
 * @param property    截面属性
 * @param[out] kl     单元刚度矩阵
 * @author   yangqiuyou
 * @date     2025.04.09
 */
void CfastCalculator::ComputeStiffness (const Real& L, const PFASTData& property, Matrixd& _OUT kl)
{
    const Real& KT1 {property.m_KT[0]};
    const Real& KT2 {property.m_KT[1]};
    const Real& KT3 {property.m_KT[2]};
    const Real& KR1 {property.m_KR[0]};
    const Real& KR2 {property.m_KR[1]};
    const Real& KR3 {property.m_KR[2]};

    kl = Matrixd (12, 12);
    
    //对角元素
    kl (0, 0)   = KT1;
    kl (1, 1)   = KT2;
    kl (2, 2)   = KT3;
    kl (3, 3)   = KR1;
    kl (4, 4)   = KT3 * L * L * 0.25 + KR2;
    kl (5, 5)   = KT2 * L * L * 0.25 + KR3;
    kl (6, 6)   = KT1;
    kl (7, 7)   = KT2;
    kl (8, 8)   = KT3;
    kl (9, 9)   = KR1;
    kl (10, 10) = KT3 * L * L * 0.25 + KR2;
    kl (11, 11) = KT2 * L * L * 0.25 + KR3;

    //右上角元素
    kl (0, 6)   = -KT1;
    kl (1, 5)   =  KT2 * L * 0.5;
    kl (1, 7)   = -KT2;
    kl (1, 11)  =  KT2 * L * 0.5;
    kl (2, 4)   = -KT3 * L * 0.5;
    kl (2, 8)   = -KT3;
    kl (2, 10)  = -KT3 * L * 0.5;
    kl (3, 9)   = -KR1;
    kl (4, 8)   =  KT3 * L * 0.5;
    kl (4, 10)  =  KT3 * L * L * 0.25 - KR2;
    kl (5, 7)   = -KT2 * L * 0.5;
    kl (5, 11)  =  KT2 * L * L * 0.25 - KR3;
    kl (7, 11)  = -KT2 * L * 0.5;
    kl (8, 10)  =  KT3 * L * 0.5;

    //左下角元素
    kl (6, 0)   = kl (0, 6);
    kl (5, 1)   = kl (1, 5);
    kl (7, 1)   = kl (1, 7);
    kl (11, 1)  = kl (1, 11);
    kl (4, 2)   = kl (2, 4);
    kl (8, 2)   = kl (2, 8);
    kl (10, 2)  = kl (2, 10);
    kl (9, 3)   = kl (3, 9);
    kl (8, 4)   = kl (4, 8);
    kl (10, 4)  = kl (4, 10);
    kl (7, 5)   = kl (5, 7);
    kl (11, 5)  = kl (5, 11);
    kl (11, 7)  = kl (7, 11);
    kl (10, 8)  = kl (8, 10);
}

void CfastCalculator::ComputeQuadWeights (const Real& x, const Real& y, const Matrixd& connectLocCoord, std::vector<Real>& _OUT weights)
{
    IntegPoint integ;
    Matrix auxLocal(1, 2, {x, y});
    SG::FEMSolver::Element::quad4ComputeIntegCoord (auxLocal, connectLocCoord, _OUT integ.m_x, _OUT integ.m_y);

    // 修正权系数
    auto tmp1 = fabs(integ.m_x - 1.0);
    auto tmp2 = fabs(integ.m_y - 1.0);

    weights.resize(4);
    weights[0] = tmp1 * tmp2 / 4.0;
    weights[1] = (2 - tmp1) * tmp2 / 4.0;
    weights[2] = (2 - tmp1) * (2 - tmp2) / 4.0;
    weights[3] = tmp1 * (2 - tmp2) / 4.0;
}

void CfastCalculator::ComputeTriWeights (const Real& x, const Real& y, const Matrixd& connectLocCoord, std::vector<Real>& _OUT weights)
{
    weights.resize(3);
    Vector3D normal;
    Real A  = SG::Algebra::calc2DTriArea(connectLocCoord , normal);

    SG::Algebra::Matrix plane1 (3 , 2 ,{ connectLocCoord (0, 0), connectLocCoord (0, 1),connectLocCoord (1, 0), connectLocCoord (1, 1), x, y});
    const Real A1 = SG::Algebra::calc2DTriArea(plane1 , normal);
    SG::Algebra::Matrix plane2 (3 , 2 ,{ connectLocCoord (0, 0), connectLocCoord (0, 1),connectLocCoord (2, 0), connectLocCoord (2, 1), x, y});
    const Real A2 = SG::Algebra::calc2DTriArea(plane2 , normal);
    
    weights[0] = (A - A1 - A2) / A;
    weights[1] = A2 / A;
    weights[2] = A1 / A;
}

//辅助点和连接壳节点位移形成 RBE3
void RBE3Calc (Id refGridId, Real xLocal, Real yLoacl, const Matrix& shellLocal, const PointArray& masterNodes, const Point& xm, const std::vector<std::size_t>& nodeIndexSet, const IdArray& dispCoords)
{
    // calc weight
    std::vector<Real> weights;
    auto shellsize = shellLocal.GetRowSize();
    switch (shellsize)
    {
        case 3:
            CfastCalculator::ComputeTriWeights (xLocal, yLoacl, shellLocal, _OUT weights);
            break;
        case 4:
            CfastCalculator::ComputeQuadWeights (xLocal, yLoacl, shellLocal, _OUT weights);
            break;
        default:
            break;
    }

    static const DofSet TransdofSet{ DOFDirections::DOF_Ux, DOFDirections::DOF_Uy, DOFDirections::DOF_Uz };

    auto&       assembly       = AssemblyEigen::GetInstance ();
    SG::DataStructure::FEM::RBE3Data rbe3;
    rbe3.m_refg = refGridId;
    rbe3.m_refc = 123;
  
    std::size_t nNode = masterNodes.size ();
    std::size_t nConnectedDof = 3 * nNode;
    Id                    cIdM = 0;
    std::vector<PointArray> xMasters;
    std::vector<IdArray> cIdNs(nNode);
    for (std::size_t iNode{0}; iNode < nNode; ++iNode)
    {
        rbe3.m_WTCG.push_back (RBE3Data::WTCG{weights[iNode], 123, {0}}); // 并不需要使用当前组的连接节点编号，此处写入0占位
        xMasters.push_back (PointArray{masterNodes[iNode]});
        cIdNs[iNode] = IdArray{dispCoords[iNode]};
    }
    
    // 特征长度计算
    Real lc{0.0};
    for (const auto& gConnect : masterNodes)
    {
        lc += SG::Algebra::length (gConnect, xm);
    }
    lc = lc * 0.25;
    // 此时lc 长度不能为0.0
    if (lc < 1.0e-6)
    {
        SGFatal << "RBE3 lc equal to zero";
    }

    SG::Algebra::Matrix RnRm;
    RBE3Calculator::CalcControlEquation (rbe3, xm,  cIdM, xMasters, cIdNs, lc, nConnectedDof, _OUT RnRm);
    
    std::vector<int>        mSetGlobalId;
    assembly.ComputeGlobalDOFId (rbe3.m_refg, rbe3.GetREFDOF (), _OUT mSetGlobalId);
    std::vector<int>        nSetGlobalId;
    assembly.GetGlobalDOFId (nodeIndexSet, TransdofSet, _OUT nSetGlobalId);

    assembly.AccumulateControlMatrix (nSetGlobalId, mSetGlobalId, RnRm);
    for (auto& gm : mSetGlobalId)
    {
        assembly.AccumulateControlMatrix(gm, gm, -1.0);
    }
}

void CfastCalculator::ComputeElementMatries (const DBServiceFactorySP dbServiceFactory)
{
    auto&       coordHandler   = CoordinateProcessor::GetInstance ();
    auto&       assembly       = AssemblyEigen::GetInstance ();
    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();
    const auto  eleRepository  = dbServiceFactory->get<IElementService> ();

    // 截面计算，获取厚度
    ConnectElementHleper connectHleper;

    static const DofSet dofSet{ DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz,
                                DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz };
    static const DofSet TransdofSet{ DOFDirections::DOF_Ux, DOFDirections::DOF_Uy, DOFDirections::DOF_Uz };

    std::vector<int>         dofGlobalIndexSet (12);
    std::vector<std::size_t> nodeIndexSet (2);
    // 单元循环 
    SG::DataStructure::FEM::CFASTData eleData;
    Matrixd   kg (12, 12);
    Point     gS;

    // 商飞客户要求， CFAST不使用辅助点
    bool customized{false};
    const auto& config = SG::Utility::SGConfig::GetInstance ().GetData ();
    if (config.Has("CFAST"))
    {
        auto FASTSetting = config.GetValue("CFAST");
        // std::cout << FASTSetting.PrettyPrintJsonString();
        customized = FASTSetting.GetValue("SFCustomized").GetBool();
    }

    PointArray                         gASet;
    PointArray                         gBSet;
    std::vector<std::size_t>           nodeIndexSetA;
    std::vector<std::size_t>           nodeIndexSetB;
    SG::DataStructure::Common::IdArray dispCoordsA;
    SG::DataStructure::Common::IdArray dispCoordsB;
    
    // 连接节点的Basic坐标 及 位移输出坐标系
    Point    GA , GB;
    Id gAdispCoodId{0};
    Id gBdispCoodId{0};

    auto      eleSize = eleRepository->Size<SG::DataStructure::FEM::CFASTData>();
    for (std::size_t index{ 0 }; index < eleSize; ++index)
    {
        eleRepository->FindByIndex (index, _OUT eleData);

        // 长度为0.0，需要使用壳厚度
        Real tA{0.0};
        Real tB{0.0};
        Vector3D nA;
        Point    pointGS;
        Id       dispCoordsGS;
        NodeData node;
        // Task0 考虑Nastran前处理先得到投影，给定GA和GB的情况
        if (eleData.m_g[0] < 101000000 && eleData.m_g[1] < 101000001)
        {
            nodeRepository ->FindById (eleData.m_g[0], _OUT node);
            coordHandler.GetGlobalCoordinate (node, _OUT GA, _OUT gAdispCoodId);
            nodeRepository ->FindById (eleData.m_g[1], _OUT node);
            coordHandler.GetGlobalCoordinate (node, _OUT GB, _OUT gBdispCoodId);
        }

        // 获取投影节点
        if (eleData.m_gs > 0)
        {
            nodeRepository ->FindById (eleData.m_gs, _OUT node);
            coordHandler.GetGlobalCoordinate (node, _OUT pointGS, _OUT dispCoordsGS);
        }
        else
        {
            // 使用 XS YS ZS
            pointGS.m_x = eleData.m_xs;
            pointGS.m_y = eleData.m_ys;
            pointGS.m_z = eleData.m_zs;
        }
        // 找到投影面单元编号， 面属性号
        Id ShellIdA, ShellIdB;
        Id pIdA{0}, pIdB{0};


        auto CfastFormat = eleData.GetType();
        switch (CfastFormat)
        {
            case CFASTData::CFASTTypes::CFASTType_Prop:
            {
                pIdA = eleData.m_ida;
                pIdB = eleData.m_idb;

                // Task1.3.1 找到属性号为PID单元。
                IdArray validShellIdsA = eleRepository->FindElementByPropId (pIdA);
                IdArray validShellIdsB = eleRepository->FindElementByPropId (pIdB);
             
                // Task1.3.3 找投影点和单元。
                Point refP1 = (eleData.m_gs > 0)? pointGS : GA;
                Point refP2 = (eleData.m_gs > 0)? pointGS : GB;

                // Task1.3.2 计算GS点到其中距离最小的单元。
                CweldCalculator::FindNearestElement(dbServiceFactory, refP1, validShellIdsA, _OUT ShellIdA);
                CweldCalculator::FindNearestElement(dbServiceFactory, refP2, validShellIdsB, _OUT ShellIdB);
                break;
            }
            case CFASTData::CFASTTypes::CFASTType_Elem:
            {
                ShellIdA = eleData.m_ida;
                ShellIdB = eleData.m_idb;

                pIdA = eleRepository -> QueryShellPropId (eleData.m_ida);
                pIdB = eleRepository -> QueryShellPropId (eleData.m_idb);

                break;
            }
            default:
                break;
        }
        //Task2：输出投影点 GA,GB

        auto nodelist = eleRepository ->FindElementNodes (ShellIdA);
        ModelInfoQuery::QueryNode (nodeRepository, nodelist.data(), nodelist.size (), gASet, nodeIndexSetA, dispCoordsA);
        nodelist = eleRepository ->FindElementNodes (ShellIdB);
        ModelInfoQuery::QueryNode (nodeRepository, nodelist.data(), nodelist.size (), gBSet, nodeIndexSetB, dispCoordsB);

        // 投影点为虚节点，需要通过投影计算GA GB 的basic 坐标
        if (eleData.m_g[0] > 101000000 || eleData.m_g[1] > 101000000)
        {
            // GS已知的情况下，前面已经求出投影单元，现在进行投影。
            CweldCalculator::ComputeProjectionPoint2 ( pointGS,  _OUT gASet, _OUT gBSet, _OUT GA, _OUT GB, _OUT nA);
        }

        Real L = length (GA, GB);
        // 查询壳厚度， 节点重合特别处理
        if (L < 1.0e-6)
        {
            // 查询壳截面属性Id
            tA = connectHleper.ComputeShellSecThickness (dbServiceFactory, pIdA);

            // 另一个截面
            tB = (pIdA == pIdB) ? tA : connectHleper.ComputeShellSecThickness (dbServiceFactory, pIdB);
        }

        // Get stiffness argument from PFASTData
        PFASTData         pFast;
        dbServiceFactory->get<IPFASTService> () ->FindById (eleData.m_pId, pFast);
        const Real   a = sqrt (3.1415926535897932) * pFast.m_D / 4.0;

        // 粱单元刚度矩阵计算
        Matrix  T (3, 3);
        if (L > 1.0e-6)
        {
            SG::Algebra::computeLocalCoordTrans (GA, GB, _OUT T);
            ComputeStiffness (L, pFast, _OUT kg);
        }
        // else
        // {
        //     // 梁长为零的特殊情况
        //     // 获取壳截面厚度
        //     Point point1{0.0, 0.0, 0.0};
        //     Point point2{nA.m_x, nA.m_y, nA.m_z};
        //     SG::Algebra::computeLocalCoordTrans (point1, point2, _OUT T);
        //     ComputeStiffness (Le, material, pBeam, _OUT kg);
        // }

        const auto TBeam = ElementTools::TwoNodeEleCoordTrans (T, gAdispCoodId, gBdispCoodId, GA, GB);
        kg = TBeam.Transpose () * kg * TBeam;

        // assembly
        assembly.FindNodeIndex (eleData.m_g, _OUT nodeIndexSet);
        assembly.GetGlobalDOFId (nodeIndexSet, dofSet, _OUT dofGlobalIndexSet);
        assembly.AccumulateStiffness (dofGlobalIndexSet, kg);

        /** @brief    建立投影点 和壳面之间的约束方程
         * 
         * 约束方程分成2级别， 投影点为圆心的圆柱上建立4个辅助节点。
         * 投影点和 4个辅助点（只要线位移参与约束方程） 形成 Kirchhoff 约束
         * 每个辅助点和壳节点 构成 RBE3 单元
         * @param a                 圆柱直径参数
         * @param T                 梁单元的单元局部坐标系，用来确定 辅助点位置
         * 
         * @param plane             壳节点的全局坐标
         * @param shellNodeIndexSet 壳节点索引（求解内部使用的连续索引）
         * @param shellDispCoods    壳节点的位移输出坐标系
         * 
         * @param project           投影点在Basic坐标系坐标
         * @param projectdispCoodId 投影点的位移输出坐标系
         * @param projectIndex      投影点的索引
         * @param auxNodeIds        辅助点Id，单元导入时分配[但是辅助点实际坐标在本函数中计算]
         * @author   wanghuan/yangqiuyou
         * @date     2025.05.14
         */
        auto surfaceConstraint = [&a, &assembly, &T] (const PointArray& plane, const std::vector<std::size_t>& shellNodeIndexSet, const IdArray& shellDispCoods, const Point& project, const Id& projectdispCoodId, std::size_t projectIndex, const IdArray& auxNodeIds)
        {
            // 壳单元单元局部坐标系
            Matrix Tshell;
            Point  origin;
            Matrix shellLocal;

            CweldCalculator::ComputeSurfaceCoord (plane, _OUT Tshell, _OUT origin, _OUT shellLocal);

            // 投影点在壳单元局部坐标系下的坐标
            Point  projectLocal;
            Point::Transform (Tshell.Transpose(), origin, project, _OUT projectLocal);
            
            // 4个辅助点在壳单元局部坐标系下的坐标
            Matrix auxPointLocal;
            CfastCalculator::ComputeAuxNode (projectLocal, Tshell, T, a, _OUT auxPointLocal);
            
            // 辅助点全局坐标
            SG::Algebra::PointArray auxGlobalCoord(4);
            auto T32   = SG::Algebra::Matrix (3, 2);
            T32(0, 0) = Tshell(0, 0);
            T32(0, 1) = Tshell(0, 1);
            T32(1, 0) = Tshell(1, 0);
            T32(1, 1) = Tshell(1, 1);
            T32(2, 0) = Tshell(2, 0);
            T32(2, 1) = Tshell(2, 1);

            Matrix AuxGlobal3D = auxPointLocal * T32.Transpose();

            for(auto i = 0; i < 4; ++i)
            {
                auxGlobalCoord[i].m_x = AuxGlobal3D (i, 0) + origin.m_x;
                auxGlobalCoord[i].m_y = AuxGlobal3D (i, 1) + origin.m_y;
                auxGlobalCoord[i].m_z = AuxGlobal3D (i, 2) + origin.m_z;
            }
            
            // 投影点与 4个虚拟辅助点 对应的全局自由度
            std::vector<int>        nSetGlobalId;
            std::vector<std::size_t> nodeIndexsAux(4);
            assembly.FindNodeIndex (auxNodeIds.data(), _OUT nodeIndexsAux);
            assembly.GetGlobalDOFId (nodeIndexsAux, TransdofSet, _OUT nSetGlobalId);
            
            std::vector<int>        mSetGlobalId;
            assembly.GetGlobalDOFId ({projectIndex}, dofSet, _OUT mSetGlobalId);

            // 投影点和辅助点之间的 Kirchhoff 约束
            Matrix  Rvirual;
            SG::DataStructure::Common::IdArray auxDispCoordIds (4, 0); 
            CweldCalculator::ComputeConstrantEquation (project, projectdispCoodId, auxGlobalCoord, auxDispCoordIds, _OUT Rvirual);

            assembly.AccumulateControlMatrix (nSetGlobalId, mSetGlobalId, Rvirual);
            for (auto& gdofm : mSetGlobalId)
            {
                assembly.AccumulateControlMatrix(gdofm, gdofm, -1.0);
            }

            // 辅助点和参考节点构成RBE3
            for (std::size_t size = 0; size < 4; size++)
            {                
                const auto& xm = auxGlobalCoord[size];
                RBE3Calc (auxNodeIds[size], auxPointLocal(size, 0), auxPointLocal(size, 1), shellLocal, plane, xm, shellNodeIndexSet, shellDispCoods);
            }
        };

        auto surfaceConstraint1 = [&assembly] (const PointArray& plane, const std::vector<std::size_t>& shellNodeIndexSet, const IdArray& shellDispCoods, const Point& project, const Id& projectdispCoodId, std::size_t projectIndex)
        {
            // 壳单元单元局部坐标系
            Matrix Tshell;
            Point  origin;
            Matrix shellLocal;

            CweldCalculator::ComputeSurfaceCoord (plane, _OUT Tshell, _OUT origin, _OUT shellLocal);

            // 投影点在壳单元局部坐标系下的坐标
            Point  projectLocal;
            Point::Transform (Tshell.Transpose(), origin, project, _OUT projectLocal);
            
            std::vector<int>        mSetGlobalId;
            assembly.GetGlobalDOFId ({projectIndex}, dofSet, _OUT mSetGlobalId);

            std::vector<int>        nSetGlobalId;
            assembly.GetGlobalDOFId (shellNodeIndexSet, TransdofSet, _OUT nSetGlobalId);

            // 投影点和辅助点之间的 Kirchhoff 约束
            Matrix  Rvirual;
            CweldCalculator::ComputeConstrantEquation (project, projectdispCoodId, plane, shellDispCoods, _OUT Rvirual);
            assembly.AccumulateControlMatrix (nSetGlobalId, mSetGlobalId, Rvirual);
            for (auto& gdofm : mSetGlobalId)
            {
                assembly.AccumulateControlMatrix(gdofm, gdofm, -1.0);
            }
        };

        if (customized)
        {
            // 直接建立投影点和 壳平面之前的约束方程
            surfaceConstraint1 (gASet, nodeIndexSetA, dispCoordsA, GA, gAdispCoodId, nodeIndexSet[0]);
            surfaceConstraint1 (gBSet, nodeIndexSetB, dispCoordsB, GB, gBdispCoodId, nodeIndexSet[1]);
        }
        else
        {
            IdArray          auxNodeIdsA{eleData.m_gh[0], eleData.m_gh[1], eleData.m_gh[2], eleData.m_gh[3]};
            surfaceConstraint (gASet, nodeIndexSetA, dispCoordsA, GA, gAdispCoodId, nodeIndexSet[0], auxNodeIdsA);
            IdArray          auxNodeIdsB{eleData.m_gh[4], eleData.m_gh[5], eleData.m_gh[6], eleData.m_gh[7]};
            surfaceConstraint (gBSet, nodeIndexSetB, dispCoordsB, GB, gBdispCoodId, nodeIndexSet[1], auxNodeIdsB);
        }
    }
}

void CfastCalculator::DataRecovery (const SG::DBManager::DBServiceFactorySP             dbServiceFactory,
                                    int                                                 subcaseId, 
                                    SG::DataStructure::Common::Real time,
                                    const std::vector<SG::DataStructure::Common::Real>& globalDisplacement)
{
    // SG::Algebra::Matrixd elementInternalForce;
    // ComputeElementInternalForce(dbServiceFactory, globalDisplacement, elementInternalForce);
}

void CfastCalculator::ComputeStressStiffness (const SG::DBManager::DBServiceFactorySP dbServiceFactory, int staticSubId)
{}

void CfastCalculator::ComputeElementInternalForce(const SG::DBManager::DBServiceFactorySP             dbServiceFactory,
                                                  const std::vector<SG::DataStructure::Common::Real>& globalDisplacement,
                                                  _OUT SG::Algebra::Matrixd&                          elementInternalForce)
{
//     1. 通过GA GB 节点的全局自由度编号，获取两个节点各自的6个位移分量【此时位移分量位于下】
    auto& coordHandler   = CoordinateProcessor::GetInstance ();
    auto& assembly = AssemblyEigen::GetInstance ();
    std::vector<std::size_t> nodeIndexSet (2);
    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();
    const auto  eleRepository  = dbServiceFactory->get<IElementService> ();
    SG::DataStructure::FEM::CWELDData eleData;
    auto      eleSize = eleRepository->Size<SG::DataStructure::FEM::CWELDData>();
    std::vector<int>         dofGlobalIndexSet (12);

    Matrixd kg;
    Real tA{0.0};
    Real tB{0.0};

    for (std::size_t index{ 0 }; index < eleSize; ++index)
    {
        static const DofSet dofSet{ DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz,
                                DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz };

        eleRepository->FindByIndex (index, _OUT eleData);
        assembly.FindNodeIndex (eleData.m_g, _OUT nodeIndexSet);
        assembly.GetGlobalDOFId (nodeIndexSet, dofSet, _OUT dofGlobalIndexSet);

        CfastCalculator cfastcalulator;
        cfastcalulator.DataRecovery (dbServiceFactory, 0, 0.0, globalDisplacement);
        Matrixd globaldisplacement{12, 1};
        for (int i = 0; i < 12; i++)
        {
            globaldisplacement(i, 0) = globalDisplacement[dofGlobalIndexSet[i]];
        }

        //获取得到GA.GB坐标
        Matrixd T(3, 3);
        NodeData node;
        Point GA, GB;
        Id dispCoordGA{0};
        Id dispCoordGB{0};

        nodeRepository ->FindById (nodeIndexSet[0], _OUT node);
        coordHandler.GetGlobalCoordinate (node, _OUT GA, _OUT dispCoordGA);
        nodeRepository ->FindById (nodeIndexSet[1], _OUT node);
        coordHandler.GetGlobalCoordinate (node, _OUT GB, _OUT dispCoordGB);
        //得到局部坐标转换矩阵
        SG::Algebra::computeLocalCoordTrans (GA, GB, _OUT T);
        Matrixd TBeam = ElementTools::TwoNodeEleCoordTrans (T, dispCoordGA, dispCoordGB, GA, GB);                                                     
    
// 2. 将节点位移转换到 单元局部坐标系下
        Matrixd elementDisplacement = TBeam * globaldisplacement;

// 3. 获取单元在局部坐标系下的梁刚度矩阵
        Real L = length (GA, GB);
        const bool zeroLenCase = (L < 1.0e-6);
        Real Le;   //等效长度
        // compute beam section property
        PBARData          pBar;
        IsotropicMaterial material;

        PFASTData pFast;
        dbServiceFactory->get<IPFASTService> ()->FindById (eleData.m_pId, pFast);
        dbServiceFactory->get<IIsotropicMaterialService> ()->FindById (pFast.m_MID, _OUT material);

        Matrix Te (3, 3);
        if (L > 1.0e-6)
        {
            SG::Algebra::computeLocalCoordTrans (GA, GB, _OUT Te);
            cfastcalulator.ComputeStiffness (L, pFast, _OUT kg);
        }
        
        // 4. 利用 $$ F = K U$$计算内力
        elementInternalForce = kg * elementDisplacement;
    }
}